package MidterRobot;

import java.io.DataInputStream;
import java.io.DataOutputStream;
import java.io.IOException;

import lejos.nxt.*;
import lejos.nxt.comm.BTConnection;
import lejos.nxt.comm.Bluetooth;
import lejos.nxt.comm.NXTConnection;

public class MidterRobotMain {

	public static final int MOTOR_A_C_STOP = 11;
	public static final int MOTOR_A_C_FORWARD = 1;
	public static final int MOTOR_A_C_BACKWARD = 2;
	public static final int MOTOR_A_FORWARD = 3;
	public static final int MOTOR_A_BACKWARD = 4;
	public static final int MOTOR_C_FORWARD = 5;
	public static final int MOTOR_C_BACKWARD = 6;    
	public static final int MOTOR_C_STOP = 7;    
	public static final int MOTOR_A_STOP = 8;    
	public static final int POWER_UP=9;
	public static final int POWER_DOWN=10;
	public static final int DISCONNECT = 99; 

	public static void main(String [] args) throws IOException, InterruptedException
	{  	    
		int speed = 500;
		int MotorAState = 1;
		int MotorCState = 1;

		Button.ESCAPE.addButtonListener(new ButtonListener() {
			@Override public void buttonReleased(Button b) { System.exit(0); }
			@Override public void buttonPressed(Button b) {}
		});

		String connected = "Connected";
		String waiting = "Waiting...";
		String closing = "Closing...";  

		while (true)
		{
			LCD.drawString(waiting,0,0);
			LCD.refresh();

			BTConnection btc = Bluetooth.waitForConnection(0, NXTConnection.RAW);

			LCD.clear();
			LCD.drawString(connected,0,0);
			LCD.refresh();    

			DataInputStream dis = btc.openDataInputStream();
			DataOutputStream dos = btc.openDataOutputStream();

			while(true)
			{
				int command = (int) dis.readByte();

				switch (command) {
				case MOTOR_A_FORWARD: 
					Motor.A.forward();
					LCD.clear();
					LCD.drawString("Forward, power: " + speed, 0, 0);
					break;
				case MOTOR_A_BACKWARD: 
					Motor.A.backward();
					LCD.clear();
					LCD.drawString("Bacward, power: " + speed, 0, 0);
					break;
				case MOTOR_A_STOP: 
					Motor.A.stop();
					LCD.clear();
					LCD.drawString("Stop, power: " + speed, 0, 0);
					break;
				case MOTOR_C_FORWARD: 
					MotorCState = 2;
					LCD.clear();
					LCD.drawString("Forward, power: " + speed, 0, 0);
					break;
				case MOTOR_C_BACKWARD: 
					Motor.C.backward();
					LCD.clear();
					LCD.drawString("Bacward, power: " + speed, 0, 0);
					break;
				case MOTOR_C_STOP: 
					Motor.C.stop();
					LCD.clear();
					LCD.drawString("Stop, power: " + speed, 0, 0);
					break;
				case MOTOR_A_C_FORWARD:
					Motor.A.forward();
					Motor.C.forward();
					LCD.clear();
					LCD.drawString("Forward, power: " + speed, 0, 0);
					break;
				case MOTOR_A_C_BACKWARD:
					Motor.A.backward();
					Motor.C.backward();
					LCD.clear();
					LCD.drawString("Bacward, power: " + speed, 0, 0);
					break;
				case MOTOR_A_C_STOP:
					Motor.A.stop();
					Motor.C.stop();
					LCD.clear();
					LCD.drawString("Stop, power: " + speed, 0, 0);
					break;
				case POWER_DOWN:
					if(speed > 0)
					{
						Motor.A.setSpeed(speed -= 100);
						Motor.B.setSpeed(speed);
					}
					LCD.clear();
					LCD.drawString("Down, power: " + speed, 0, 0);
					break;
				case POWER_UP:
					if(speed < 900)
					{						
						Motor.A.setSpeed(speed += 100);
						Motor.B.setSpeed(speed);
					}
					LCD.clear();
					LCD.drawString("Up, power: " + speed, 0, 0);
					break;
				case DISCONNECT: 
					LCD.clear();
					LCD.drawString("Disconnect, power: " + speed, 0, 0);
					break;
				default: 
					LCD.clear();
					LCD.drawString("Error, power: " + speed, 0, 0);                     
				}
				if (command == DISCONNECT) break;
				LCD.drawString("Command: " + command, 0, 1);

				//dos.writeByte(MotorAState + MotorCState);
				LCD.drawString("MotA: " + Motor.A.getMode(), 0, 2);
				LCD.drawString("MotB: " + Motor.B.getMode(), 0, 3);

			}

			dis.close();
			dos.close();
			Thread.sleep(100); // wait for data to drain
			LCD.clear();
			LCD.drawString(closing,0,0);
			LCD.refresh();
			btc.close();
			LCD.clear();
		}

	}

}
